/*
 * Copyright (c) 2020 Intel Corporation
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * @file motion_kernel.hpp
 *
 * Maintainer: Yu Yan <yu.yan@intel.com>
 *
 */

#pragma once

#include <RTmotion/global.hpp>
#include <queue>
#include <RTmotion/fb/execution_node.hpp>

namespace RTmotion
{
class ExecutionNode;

class MotionKernel
{
public:
  MotionKernel();

  virtual ~MotionKernel();

  virtual void runCycle(const double t, const double t_prev, const double pos, const double vel, MC_AXIS_STATES* state);

  void addFBToQueue(ExecutionNode* fb, LREAL current_pos, LREAL current_vel);

  std::queue<ExecutionNode*>& getQueuedMotions();

  void getCommands(double* pos_cmd, double* vel_cmd, double duration);

  void setAllFBsAborted();

private:
  std::queue<ExecutionNode*> fb_queue_;
  ExecutionNode* fb_hold_ = nullptr;
};

}  // namespace RTmotion